package Physics;


/**
 *  wrapper class that holds fine-collision detection methods
 *  each function takes two objects and contact data
 * @author G
 *
 */
public class CollisionDetector
{
	public static int sphereAndHalfSpace(CollisionSphere sphere, CollisionPlane plane, CollisionData data)
	{
		// Make sure we have contacts
		if (data.m_contactsLeft <= 0)
			return 0;

		// cache the sphere position
		Vector3 position = sphere.getAxis(3);

		// find the distance from the plane
		float ballDistance = plane.m_direction.dotProduct(position)
				- sphere.m_radius - plane.m_distance;

		if (ballDistance >= 0)
			return 0;

		// create the contact
		//Contact contact = data.m_contactArray;
		Contact contact = new Contact();
		contact.m_contactNormal = plane.m_direction;
		contact.m_penetration = -ballDistance;
		contact.m_contactPoint = position.subtract(plane.m_direction
				.multiply((ballDistance + sphere.m_radius)));
		contact.setBodyData(sphere.m_body, null, data.m_friction,
				data.m_restitution);
		data.addContact(contact);
		return 1;

	}

	public static int sphereAndTruePlane(CollisionSphere sphere, CollisionPlane plane, CollisionData data)
	{
		// make sure we have contacts left
		if (data.m_contactsLeft <= 0)
			return 0;

		// cache the sphere's position
		Vector3 position = sphere.getAxis(3);

		// find the distance from the plane
		float centerDistance = plane.m_direction.dotProduct(position)
				- plane.m_distance;

		// check if we're outside the radius of the sphere
		if (centerDistance * centerDistance > sphere.m_radius * sphere.m_radius) {
			return 0;
		}

		// check which side of the plane we're on
		Vector3 normal = plane.m_direction;
		float penetration = -centerDistance;

		if (centerDistance < 0) {
			// normal *= -1;
			normal.invert(); // ??
			penetration = -penetration;
		}
		penetration += sphere.m_radius;

		// create the contact information and add it for processing
		//Contact contact = data.m_contacts;
		Contact contact = new Contact();
		contact.m_contactNormal = normal;
		contact.m_penetration = penetration;
		contact.m_contactPoint = position.subtract(plane.m_direction
				.multiply(centerDistance));
		contact.setBodyData(sphere.m_body, null, data.m_friction,
				data.m_restitution);
		data.addContact(contact);
		return 1;

	}

	public static int sphereAndSphere(CollisionSphere one, CollisionSphere two, CollisionData data)
	{
		// Make sure we have contacts
		if (data.m_contactsLeft <= 0)
			return 0;

		// Cache the sphere positions
		Vector3 positionOne = one.getAxis(3);
		Vector3 positionTwo = two.getAxis(3);

		// Find the vector between the objects
		Vector3 midline = positionOne.subtract(positionTwo);
		float size = midline.magnitude();

		// See if it is large enough.
		if (size <= 0.0f || size >= one.m_radius + two.m_radius) {
			return 0;
		}

		// We manually create the normal, because we have the
		// size to hand.
		Vector3 normal = midline.multiply((((float) 1.0) / size));

		//Contact contact = data.m_contacts;
		Contact contact = new Contact();
		contact.m_contactNormal = normal;
		contact.m_contactPoint = positionOne.add(midline).multiply((float) 0.5);
		contact.m_penetration = (one.m_radius + two.m_radius - size);
		contact.setBodyData(one.m_body, two.m_body, data.m_friction,
				data.m_restitution);

		data.addContact(contact);
		return 1;

	}

	public static int boxAndHalfSpace(CollisionBox box, CollisionPlane plane, CollisionData data) 
	{
		if (!box.m_body.hasFiniteMass())
			return 0;

		// Make sure we have contacts
		if (data.m_contactsLeft <= 0)
			return 0;

		// Check for intersection
		if (!IntersectionTests.boxAndHalfSpace(box, plane)) {
			return 0;
		}

		// We have an intersection, so find the intersection points. We can make
		// do with only checking vertices. If the box is resting on a plane
		// or on an edge, it will be reported as four or two contact points.

		// Go through each combination of + and - for each half-size
		float mults[][] = { { 1, 1, 1 }, { -1, 1, 1 }, { 1, -1, 1 },
				{ -1, -1, 1 }, { 1, 1, -1 }, { -1, 1, -1 }, { 1, -1, -1 },
				{ -1, -1, -1 } };

		//Contact contact = data.m_contacts;
		//Contact contact = new Contact();

		int contactsUsed = 0;

		for (int i = 0; i < 8; i++) {
			// Calculate the position of each vertex
			Vector3 vertexPos = new Vector3(mults[i][0], mults[i][1],
					mults[i][2]);
			vertexPos.componentProductSet(box.m_halfSize);
			vertexPos = box.m_transform.transform(vertexPos);

			// Calculate the distance from the plane
			float vertexDistance = vertexPos.dotProduct(plane.m_direction);

			// Compare this to the plane's distance
			if (vertexDistance <= plane.m_distance) {
				// Create the contact data.

				Contact contact = new Contact();
				// The contact point is halfway between the vertex and the
				// plane - we multiply the direction by half the separation
				// distance and add the vertex location.
				contact.m_contactPoint = new Vector3(plane.m_direction);
				contact.m_contactPoint.multiplyAndSet(vertexDistance
						- plane.m_distance);
				contact.m_contactPoint = vertexPos;
				contact.m_contactNormal = new Vector3(plane.m_direction);
				contact.m_penetration = plane.m_distance - vertexDistance;

				// Write the appropriate data
				contact.setBodyData(box.m_body, null, data.m_friction,
						data.m_restitution);

				data.addContact(contact); 
				contactsUsed++;
				
				// Move onto the next contact
				/*
				contact++;
				contactsUsed++;
				if (contactsUsed == data.m_contactsLeft)
					return contactsUsed;*/
			}
		}

		//data.addContact(contactsUsed);
		return contactsUsed;

	}

	public static int boxAndBox(CollisionBox one, CollisionBox two, CollisionData data) 
	{
		if (!one.m_body.hasFiniteMass() && !two.m_body.hasFiniteMass())
			return 0;

		 // Find the vector between the two centres
		Vector3 toCentre = two.getAxis(3).subtract(one.getAxis(3));

		 // We start assuming there is no contact
		//float pen = Float.MAX_VALUE;
		//int best = 99999;
		
		// Java hack. Create an array of size 1, in order to create
		// a reference to an int, so that the changes made in all of
		// the following tryAxis() calls will stick!
		float [] pen = new float[1];
		pen[0] = Float.MAX_VALUE;
		int [] best = new int[1];
		best[0] = Integer.MAX_VALUE;


		// Now we check each axes, returning if it gives us a separating axis,
		// and keeping track of the axis with the smallest penetration
		// otherwise.
		// CHECK_OVERLAP(one.getAxis(0), 0);
		// CHECK_OVERLAP(one.getAxis(1), 1);
		// CHECK_OVERLAP(one.getAxis(2), 2);
		if(!tryAxis(one, two, one.getAxis(0), toCentre, 0, pen, best)) return 0;
		if(!tryAxis(one, two, one.getAxis(1), toCentre, 1, pen, best)) return 0;
		if(!tryAxis(one, two, one.getAxis(2), toCentre, 2, pen, best)) return 0;
		
		// CHECK_OVERLAP(two.getAxis(0), 3);
		// CHECK_OVERLAP(two.getAxis(1), 4);
		// CHECK_OVERLAP(two.getAxis(2), 5);
		if(!tryAxis(one, two, two.getAxis(0), toCentre, 3, pen, best)) return 0;
		if(!tryAxis(one, two, two.getAxis(1), toCentre, 4, pen, best)) return 0;
		if(!tryAxis(one, two, two.getAxis(2), toCentre, 5, pen, best)) return 0;
		
		 // Store the best axis-major, in case we run into almost parallel edge
		 // collisions later
		 
		int bestSingleAxis = best[0];
		
		// CHECK_OVERLAP(one.getAxis(0).crossProduct(two.getAxis(0)), 6);
		// CHECK_OVERLAP(one.getAxis(0).crossProduct(two.getAxis(1)), 7);
		// CHECK_OVERLAP(one.getAxis(0).crossProduct(two.getAxis(2)), 8);
		// CHECK_OVERLAP(one.getAxis(1).crossProduct(two.getAxis(0)), 9);
		// CHECK_OVERLAP(one.getAxis(1).crossProduct(two.getAxis(1)), 10);
		// CHECK_OVERLAP(one.getAxis(1).crossProduct(two.getAxis(2)), 11);
		// CHECK_OVERLAP(one.getAxis(2).crossProduct(two.getAxis(0)), 12);
		// CHECK_OVERLAP(one.getAxis(2).crossProduct(two.getAxis(1)), 13);
		// CHECK_OVERLAP(one.getAxis(2).crossProduct(two.getAxis(2)), 14);
		if(!tryAxis(one, two, one.getAxis(0).crossProduct(two.getAxis(0)), toCentre, 6, pen, best)) return 0;
		if(!tryAxis(one, two, one.getAxis(0).crossProduct(two.getAxis(1)), toCentre, 7, pen, best)) return 0;
		if(!tryAxis(one, two, one.getAxis(0).crossProduct(two.getAxis(2)), toCentre, 8, pen, best)) return 0;
		if(!tryAxis(one, two, one.getAxis(1).crossProduct(two.getAxis(0)), toCentre, 9, pen, best)) return 0;
		if(!tryAxis(one, two, one.getAxis(1).crossProduct(two.getAxis(1)), toCentre, 10, pen, best)) return 0;
		if(!tryAxis(one, two, one.getAxis(1).crossProduct(two.getAxis(2)), toCentre, 11, pen, best)) return 0;
		if(!tryAxis(one, two, one.getAxis(2).crossProduct(two.getAxis(0)), toCentre, 12, pen, best)) return 0;
		if(!tryAxis(one, two, one.getAxis(2).crossProduct(two.getAxis(1)), toCentre, 13, pen, best)) return 0;
		if(!tryAxis(one, two, one.getAxis(2).crossProduct(two.getAxis(2)), toCentre, 14, pen, best)) return 0;

		 // Make sure we've got a result.
		assert (best[0] != 99999);
	
		 // We now know there's a collision, and we know which of the axes gave
		 // the smallest penetration. We now can deal with it in different ways
		 // depending on the case.
		if (best[0] < 3) {
			 // We've got a vertex of box two on a face of box one.
			fillPointFaceBoxBox(one, two, toCentre, data, best[0], pen[0]);
			//data.addContacts(1);
			return 1;
		} else if (best[0] < 6) {
			 // We've got a vertex of box one on a face of box two. We use the
			 // same algorithm as above, but swap around one and two (and
			 // therefore also the vector between their centres).
			fillPointFaceBoxBox(two, one, toCentre.multiply(-1.0f), data,
					best[0] - 3, pen[0]);
			//data.addContacts(1);
			return 1;
		} else {
			 // We've got an edge-edge contact. Find out which axes
			best[0] -= 6;
			int oneAxisIndex = best[0] / 3;
			int twoAxisIndex = best[0] % 3;
			Vector3 oneAxis = one.getAxis(oneAxisIndex);
			Vector3 twoAxis = two.getAxis(twoAxisIndex);
			Vector3 axis = oneAxis.crossProduct(twoAxis);
			axis.normalize();

			 // The axis should point from box one to box two.
			if (axis.dotProduct(toCentre) > 0)
				axis = axis.multiply(-1.0f);

			 // We have the axes, but not the edges: each axis has 4 edges
			 // parallel to it, we need to find which of the 4 for each object.
			 // We do that by finding the point in the centre of the edge. We
			 // know its component in the direction of the box's collision axis
			 // is zero (its a mid-point) and we determine which of the extremes
			 // in each of the other axes is closest.
			Vector3 ptOnOneEdge = new Vector3(one.m_halfSize);
			Vector3 ptOnTwoEdge = new Vector3(two.m_halfSize);
			for (int i = 0; i < 3; i++) {
				if (i == oneAxisIndex)
					ptOnOneEdge.setXYZ(i, 0);
				else if (one.getAxis(i).dotProduct(axis) > 0)
					ptOnOneEdge.setXYZ(i, -ptOnOneEdge.getXYZ(i));

				if (i == twoAxisIndex)
					ptOnTwoEdge.setXYZ(i, 0);
				else if (two.getAxis(i).dotProduct(axis) < 0)
					ptOnTwoEdge.setXYZ(i, -ptOnTwoEdge.getXYZ(i));
			}

			 // Move them into world coordinates (they are already oriented 
			 // correctly, since they have been derived from the axes).
			ptOnOneEdge = one.m_transform.multiplyByVector(ptOnOneEdge);
			ptOnTwoEdge = two.m_transform.multiplyByVector(ptOnTwoEdge);

			 // So we have a point and a direction for the colliding edges. We
			 // need to find out point of closest approach of the two
			 // line-segments.
			Vector3 vertex = contactPoint(ptOnOneEdge, oneAxis,
					one.m_halfSize.getXYZ(oneAxisIndex), ptOnTwoEdge, twoAxis,
					two.m_halfSize.getXYZ(oneAxisIndex), bestSingleAxis > 2);

			 // We can fill the contact.
			//Contact contact = data.m_contacts;
			Contact contact = new Contact();
			
			contact.m_penetration = pen[0];
			contact.m_contactNormal = axis;
			contact.m_contactPoint = vertex;
			contact.setBodyData(one.m_body, two.m_body, data.m_friction,
					data.m_restitution);
			//data.addContacts(1);
			data.addContact(contact);
			return 1;
		}
		//return 0;
	}

	public static int boxAndPoint(CollisionBox box, Vector3 point, CollisionData data)
	{
		if (!box.m_body.hasFiniteMass())
			return 0;

		// Transform the point into box coordinates
		Vector3 relPt = box.m_transform.transformInverse(point);

		Vector3 normal;

		// Check each axis, looking for the axis on which the
		// penetration is least deep.
		float min_depth = box.m_halfSize.x - Math.abs(relPt.x);
		if (min_depth < 0)
			return 0;

		normal = box.getAxis(0).multiply(((relPt.x < 0) ? -1 : 1));

		float depth = box.m_halfSize.y - Math.abs(relPt.y);
		if (depth < 0)
			return 0;
		else if (depth < min_depth) {
			min_depth = depth;
			normal = box.getAxis(1).multiply(((relPt.y < 0) ? -1 : 1));
		}

		depth = box.m_halfSize.z - Math.abs(relPt.z);
		if (depth < 0)
			return 0;
		else if (depth < min_depth) {
			min_depth = depth;
			normal = box.getAxis(2).multiply(((relPt.z < 0) ? -1 : 1));
		}

		// Compile the contact
		//Contact contact = data.m_contacts;
		Contact contact = new Contact();
		
		contact.m_contactNormal = normal;
		contact.m_contactPoint = point;
		contact.m_penetration = min_depth;

		// Note that we don't know what rigid body the point
		// belongs to, so we just use NULL. Where this is called
		// this value can be left, or filled in.
		contact.setBodyData(box.m_body, null, data.m_friction,
				data.m_restitution);

		//data.addContacts(1);
		data.addContact(contact);
		return 1;

	}

	public static int boxAndSphere(CollisionBox box, CollisionSphere sphere, CollisionData data)
	{
		if (!box.m_body.hasFiniteMass() && !sphere.m_body.hasFiniteMass())
			return 0;

		// Transform the centre of the sphere into box coordinates
		Vector3 centre = sphere.getAxis(3);
		Vector3 relCentre = box.m_transform.transformInverse(centre);

		// Early out check to see if we can exclude the contact
		if (Math.abs(relCentre.x) - sphere.m_radius > box.m_halfSize.x
				|| Math.abs(relCentre.y) - sphere.m_radius > box.m_halfSize.y
				|| Math.abs(relCentre.z) - sphere.m_radius > box.m_halfSize.z) {
			return 0;
		}

		Vector3 closestPt = new Vector3(0, 0, 0);
		float dist;

		// Clamp each coordinate to the box.
		dist = relCentre.x;
		if (dist > box.m_halfSize.x)
			dist = box.m_halfSize.x;
		if (dist < -box.m_halfSize.x)
			dist = -box.m_halfSize.x;
		closestPt.x = dist;

		dist = relCentre.y;
		if (dist > box.m_halfSize.y)
			dist = box.m_halfSize.y;
		if (dist < -box.m_halfSize.y)
			dist = -box.m_halfSize.y;
		closestPt.y = dist;

		dist = relCentre.z;
		if (dist > box.m_halfSize.z)
			dist = box.m_halfSize.z;
		if (dist < -box.m_halfSize.z)
			dist = -box.m_halfSize.z;
		closestPt.z = dist;

		// Check we're in contact
		dist = (closestPt.subtract(relCentre).squareMagnitude());
		if (dist > sphere.m_radius * sphere.m_radius)
			return 0;

		// Compile the contact
		Vector3 closestPtWorld = box.m_transform.transform(closestPt);

		//Contact contact = data.m_contacts;
		Contact contact = new Contact();
		contact.m_contactNormal = (closestPtWorld.subtract(centre));
		contact.m_contactNormal.normalize();
		contact.m_contactPoint = closestPtWorld;
		contact.m_penetration = (float) (sphere.m_radius - Math.sqrt(dist));
		contact.setBodyData(box.m_body, sphere.m_body, data.m_friction,
				data.m_restitution);

		//data.addContacts(1);
		data.addContact(contact);
		return 1;

	}

	static void fillPointFaceBoxBox(CollisionBox one, CollisionBox two,
									Vector3 toCentre, CollisionData data, int best, float pen)
	{
		// This method is called when we know that a vertex from
		// box two is in contact with box one.

		//Contact contact = data.m_contacts;
		Contact contact = new Contact();

		// We know which axis the collision is on (i.e. best),
		// but we need to work out which of the two faces on
		// this axis.
		Vector3 normal = one.getAxis(best);
		if (one.getAxis(best).dotProduct(toCentre) > 0) {
			normal.multiplyAndSet(-1.0f);
		}

		// Work out which vertex of box two we're colliding with.
		// Using toCentre doesn't work!
		Vector3 vertex = new Vector3(two.m_halfSize);
		if (two.getAxis(0).dotProduct(normal) < 0)
			vertex.x = -vertex.x;
		if (two.getAxis(1).dotProduct(normal) < 0)
			vertex.y = -vertex.y;
		if (two.getAxis(2).dotProduct(normal) < 0)
			vertex.z = -vertex.z;

		// Create the contact data
		contact.m_contactNormal = normal;
		contact.m_penetration = pen;
		contact.m_contactPoint = two.getTransform().multiplyByVector(vertex);
		contact.setBodyData(one.m_body, two.m_body, data.m_friction,
				data.m_restitution);
		
		data.addContact(contact);
	}

	/**	
	 * @param useOne
	 * If this is true, and the contact point is outside the edge (in the case
	 * of an edge-face contact) then we use one's midpoint, otherwise we use two's.
	 */
	static Vector3 contactPoint(Vector3 pOne, Vector3 dOne, float oneSize,
			Vector3 pTwo, Vector3 dTwo, float twoSize, boolean useOne)
	{
		Vector3 toSt, cOne, cTwo;
		float dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo;
		float denom, mua, mub;

		smOne = dOne.squareMagnitude();
		smTwo = dTwo.squareMagnitude();
		dpOneTwo = dTwo.dotProduct(dOne);

		toSt = pOne.subtract(pTwo);
		dpStaOne = dOne.dotProduct(toSt);
		dpStaTwo = dTwo.dotProduct(toSt);

		denom = smOne * smTwo - dpOneTwo * dpOneTwo;

		 //  Zero denominator indicates parallel lines
		if (Math.abs(denom) < 0.0001f) {
			return useOne ? pOne : pTwo;
		}

		mua = (dpOneTwo * dpStaTwo - smTwo * dpStaOne) / denom;
		mub = (smOne * dpStaTwo - dpOneTwo * dpStaOne) / denom;

		// If either of the edges has the nearest point out
		// of bounds, then the edges aren't crossed, we have
		// an edge-face contact. Our point is on the edge, which
		// we know from the useOne parameter.
		if (mua > oneSize || mua < -oneSize || mub > twoSize || mub < -twoSize) {
			return useOne ? pOne : pTwo;
		} else {
			cOne = pOne.add(dOne.multiply(mua));
			cTwo = pTwo.add(dTwo.multiply(mub));

			return cOne.multiply((float)0.5).add(cTwo.multiply((float)0.5));
		}
	}

	static boolean tryAxis(CollisionBox one, CollisionBox two, Vector3 axis, 
							Vector3 toCentre, int index,
							// These values may be updated
							float [] smallestPenetration,int [] smallestCase)
		{
		    // Make sure we have a normalized axis, and don't check almost parallel axes
		    if (axis.squareMagnitude() < 0.0001) 
				return true;

		    axis.normalize();

		    float penetration = penetrationOnAxis(one, two, axis, toCentre);

		    if (penetration < 0) 
				return false;

		    if (penetration < smallestPenetration[0]) 
			{
		        smallestPenetration[0] = penetration;
		        smallestCase[0] = index;
		    }
		    return true;
		}
	
	static float penetrationOnAxis(CollisionBox one, CollisionBox two,
									Vector3 axis, Vector3 toCentre)
		{
		    // Project the half-size of one onto axis
		    float oneProject = transformToAxis(one, axis);
		    float twoProject = transformToAxis(two, axis);

		    // Project this onto the axis
		    float distance = Math.abs(toCentre.dotProduct(axis));

		    // Return the overlap (i.e. positive indicates
		    // overlap, negative indicates separation).
		    return oneProject + twoProject - distance;
		}

	static float transformToAxis(CollisionBox box, Vector3 axis)
	{
		// separating axis test. Project the three axes of the box onto the given axis
		return
	        box.m_halfSize.x * Math.abs(axis.dotProduct(box.getAxis(0))) +
	        box.m_halfSize.y * Math.abs(axis.dotProduct(box.getAxis(1))) +
	        box.m_halfSize.z * Math.abs(axis.dotProduct(box.getAxis(2)));
	}

}
